#include "g_code_handler.h"
#define USE_PC_DEBUG
#ifdef USE_PC_DEBUG
#include <stdio.h>
#endif
g_code_command_t command_list[12];

#define G_COMMAND_REGISTER_SIMPLE_INTEGER(command, src, target, param) \
if(((target->flags) & G_CODE_FLAGS_##command##_BIT) != G_CODE_FLAGS_##command##_BIT) \
{ \
    target->param = src->i_value.value; \
    target->flags |= G_CODE_FLAGS_##command##_BIT; \
} \
else \
{ \
    target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT; \
} \

#define G_COMMAND_REGISTER_SIMPLE_FLOAT(command, src, target, param) \
if(((target->flags) & G_CODE_FLAGS_##command##_BIT) != G_CODE_FLAGS_##command##_BIT) \
{ \
    target->param = src->f_value; \
    target->flags |= G_CODE_FLAGS_##command##_BIT; \
} \
else \
{ \
    target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT; \
} \

static inline void command_to_param_handler(g_code_command_t *src, g_code_parm_t *target)
{
    switch (src->cmd)
    {
    case 'G': 
    if(((target->flags) & G_CODE_FLAGS_G_BIT) != G_CODE_FLAGS_G_BIT)
    {
        target->g = src->i_value.value;
        target->flags |= G_CODE_FLAGS_G_BIT;
        if(src->i_value.sub_value & 0x8000) //G Type Sub-Group used for 5-Axis platform
        {
            target->sub_g = (src->i_value.sub_value) & 0x7fff;
            target->flags |= G_CODE_FLAGS_G_SUB_GROUP_BIT;
        }
    }
    else
    {
        target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT;
    }
    break;
    case 'M': 
        G_COMMAND_REGISTER_SIMPLE_INTEGER(M,src,target,m);
    break;
    case 'T': 
        G_COMMAND_REGISTER_SIMPLE_INTEGER(T,src,target,t);
    break;
    case 'X': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(X,src,target,x);
    break;
    case 'Y': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(Y,src,target,y);
    break;
    case 'Z': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(Z,src,target,z);
    break;
    case 'I': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(I,src,target,i);
    break;
    case 'J': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(J,src,target,j);
    break;
    case 'K': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(K,src,target,k);
    break;
    case 'R': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(R,src,target,r);
    break;
    case 'F': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(F,src,target,f);
    break;
    case 'S': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(S,src,target,s);
    break;
    default:
        break;
    }
}

cx_uint32_t g_code_get_param(char *g_code_line, g_code_parm_t *ret_param)
{
    cx_uint8_t command_num;
    ret_param->flags = 0;
    command_num = g_code_get_command(g_code_line, command_list);
    for(int i = 0; i < command_num; i++)
    {
        command_to_param_handler(&(command_list[i]), ret_param);
    }
    return command_num;
}

#ifdef USE_PC_DEBUG
int main(void)
{
    g_code_parm_t g_code_;
    char *g_code_line_ = "N1189 X18.171 Y12.184 Z13.598";
    g_code_get_param(g_code_line_,&g_code_);
    printf("Code:%s\nflags:%x\n", g_code_line_,g_code_.flags);
    printf("G:%d,SubG:%d,X:%f,Y:%f,Z:%f,F:%f\n", g_code_.g, (g_code_.flags & G_CODE_FLAGS_G_SUB_GROUP_BIT)?g_code_.sub_g:-1, g_code_.x, g_code_.y, g_code_.z,g_code_.f);
}
#endif